#pragma once

#include <vector>
#include <cassert>

template<typename T>
class MinHeap {
public:
    MinHeap() {
        nodes = std::vector<T>();
        nodes.reserve(20);
    }

    ~MinHeap() { }

    bool push(T&& node) {
        nodes.push_back(node);
        std::size_t index = nodes.size() - 1;
        assert(index >= 0);
        shift_up(index);
        return true;
    }

    bool try_pop(T& t) {
        std::size_t size = nodes.size();

        if (size == 1) {
            t = nodes.front();
            nodes.clear();
            return true;
        } else if (size > 0) {
            t = std::move(nodes.front());
            std::swap(nodes[0], nodes[size-1]);
            nodes.pop_back();
            shift_down(0);
            return true;
        }

        return false;
    }

protected:
    void shift_down(std::size_t index) {
        std::size_t left = UINT32_MAX;
        std::size_t right = UINT32_MAX;
        std::size_t size  = nodes.size();
        do {
            left = index * 2 + 1;
            right = index * 2 + 2;
            if (left < size && right < size) {
                if (nodes[left] < nodes[right]) {
                    if (nodes[left] < nodes[index]) {
                        std::swap(nodes[left] , nodes[index]);
                        index = left;
                        continue;
                    }
                } else {
                    if (nodes[right] < nodes[index]) {
                        std::swap(nodes[right] , nodes[index]);
                        index = right;
                        continue;
                    }
                }
            } else {
                if (left < size) {
                    if (nodes[left] < nodes[index]) {
                        std::swap(nodes[left] , nodes[index]);
                        index = left;
                        continue;
                    }
                } else if (right < size) {
                    if (nodes[right] < nodes[index]) {
                        std::swap(nodes[right] , nodes[index]);
                        index = right;
                        continue;
                    }
                }
            }
        } while (false);
    }

    void shift_up(std::size_t index) {
        if (index == 0) return;
        std::size_t parent = (index - 1) / 2;
        while (nodes[index] < nodes[parent]) {
            std::swap(nodes[index], nodes[parent]);

            if (parent == 0) {
                break;
            }

            index = parent;
            parent = (index - 1) / 2;
        }
    }
private:
    std::vector<T> nodes;
};